// Copyright (c) 2022 ChenJun
// Licensed under the Apache-2.0 License.

#include <tf2_ros/transform_listener.h>
// #include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>

#include <rclcpp/logging.hpp>
#include <rclcpp/qos.hpp>
#include <rclcpp/utilities.hpp>
// #include <serial_driver/serial_driver.hpp>
#if defined(ROS_DISTRO_FOXY)
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#elif defined(ROS_DISTRO_HUMBLE)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#error "Unsupported or Unknown ROS Distro"
#endif
// C++ system
#include <cstdint>
#include <functional>
#include <map>
#include <memory>
#include <string>
#include <vector>
#include <cmath>
#include "anti_spinner/anti_spinner.hpp"


#include "auto_aim_interfaces/msg/dangle_shoot.hpp"
#include "std_msgs/msg/u_int16.hpp"


#define CALIBERATING_GUN

namespace anti_spinner
{
AntiSpinner::AntiSpinner(const rclcpp::NodeOptions & options)
// : Node("anti_spinner", options),
: Node("anti_spinner", options)
  // owned_ctx_{new IoContext(2)},
  // serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx_)}
{
  RCLCPP_INFO(get_logger(), "Start AntiSpinner!");

  getParams();

  tf_buffer_ =
    std::make_unique<tf2_ros::Buffer>(this->get_clock());
  tf_listener_ =
    std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);


  // TF broadcaster
  timestamp_offset_ = this->declare_parameter("timestamp_offset", 0.0);
  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

  // Create Publisher
  latency_pub_ = this->create_publisher<std_msgs::msg::Float64>("latency", 10);
  marker_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("aiming_point", 10);

  // Detect parameter client
  detector_param_client_ = std::make_shared<rclcpp::AsyncParametersClient>(this, "armor_detector");

  // Tracker reset service client
  reset_tracker_client_ = this->create_client<std_srvs::srv::Trigger>("tracker/reset");

  aiming_point_.header.frame_id = "odom";
  aiming_point_.ns = "aiming_point";
  aiming_point_.type = visualization_msgs::msg::Marker::SPHERE;
  aiming_point_.action = visualization_msgs::msg::Marker::ADD;
  aiming_point_.scale.x = aiming_point_.scale.y = aiming_point_.scale.z = 0.12;
  aiming_point_.color.r = 1.0;
  aiming_point_.color.g = 1.0;
  aiming_point_.color.b = 1.0;
  aiming_point_.color.a = 1.0;
  aiming_point_.lifetime = rclcpp::Duration::from_seconds(0.1);

  // Create Subscription
  target_sub_ = this->create_subscription<auto_aim_interfaces::msg::Target>(
    "tracker/target", rclcpp::SensorDataQoS(),
    std::bind(&AntiSpinner::decideFollow, this, std::placeholders::_1));

  float_ctr_publisher_ = this->create_publisher<auto_aim_interfaces::msg::DangleShoot>("dangle_shoot", 10);
}

AntiSpinner::~AntiSpinner()
{

}


void AntiSpinner::predict_and_pub_ctr(const auto_aim_interfaces::msg::Target::SharedPtr &msg, float pipeline_process_latency_ms) {
  // pipeline_process_latency is the time from image acquisition to process done.
  // 根据msg中的信息，计算控制数据ctr
  /*
  if msg satisfies some condition...
  then set ctr to some value...  
  */

  // if(msg->tracking){
  //   if (msg->v_yaw > 1.0){
  //     packet1.dyaw = 0;
  //     packet1.dpitch = msg->velocity.z * dt;
  //     if (msg->yaw>2)
  //     Isshoot = 1;
  //   }
  //   else{
  //     float r0=msg->radius_1;//??
  //     yaw_new = msg->yaw +  msg->v_yaw * dt;
  //     packet1.dyaw = atan((msg->position.y + msg->velocity.y*dt-r0*sin(yaw_new))/(msg->position.x + msg->velocity.x*dt-r0*cos(yaw_new)))- atan((msg->position.y - r0*sin(msg->yaw))/(msg->position.x - r0*cos(msg->yaw)));
  //     packet1.dpitch = msg->velocity.z * dt;
      
  //     Isshoot = 1;
  //   }
  // }

  // final_shoot=1;
  float final_delta_yaw, final_delta_pitch;
  bool final_shoot;

  using namespace std::chrono;
  static time_point<high_resolution_clock> lasttime = std::chrono::system_clock::now();
  

/* 获取瞄准方案和瞄准位置 */
  if(msg->tracking) {

    // shoot the current target
    // const float delay_time = 0.02;
    // const float critical_omega = 6.28; // 一秒转两圈的，认为是转速过快，进行反小陀螺
    tf2::Vector3 aiming_to;
    // 一段时间后的机器人中心位置和旋转量
    float r1 = msg->radius_1;
    // calculate distance 当前时刻装甲板距离
    float curr_yaw = msg->yaw;
    float curr_armor_x = msg->position.x - r1 * cos(curr_yaw);
    float curr_armor_y = msg->position.y - r1 * sin(curr_yaw);;
    float curr_armor_z = msg->position.z;
    float dis = sqrt(pow(curr_armor_x, 2) + pow(curr_armor_y, 2) + pow(curr_armor_z, 2));
    
    float delay_time = dis / shoot_speed_ + shoot_action_delay_ + pipeline_process_latency_ms / 1000.0;
    // float delay_time = dis / shoot_speed_ + shoot_action_delay_ ;

    float x_center = msg->position.x + msg->velocity.x * delay_time;
    float y_center = msg->position.y + msg->velocity.y * delay_time;
    float yaw_armor = msg->yaw + msg->v_yaw * delay_time;
    float z_armor = msg->position.z  + msg->velocity.z * delay_time;
    // 计算装甲板位置
    float x_armor = x_center - r1 * cos(yaw_armor);
    float y_armor = y_center - r1 * sin(yaw_armor);

    bool anti_spinner = abs(msg->v_yaw) > critical_omega_;
    if(anti_spinner) {
      // 反小陀螺，瞄准机器人一段时间后所处的位置时的机器人中心
      // 瞄准机器人中心、正在追踪的装甲板的高度
      aiming_to.setX(x_center);
      aiming_to.setY(y_center);
      aiming_to.setZ(z_armor);
    } else {
      // 正常情况下，瞄准机器人一段时间后的装甲板位置
      
      // float r2 = msg->radius_2;

      // point x of the gimbal link to

      // 从原点指向瞄准点的单位向量，用tf计算
      // 设置aiming_to的值
      aiming_to.setX(x_armor);
      aiming_to.setY(y_armor);
      aiming_to.setZ(z_armor);
    }

    // 单位向量
    aiming_to.normalize();
    // z轴单位向量
    tf2::Vector3 odom_z_axis(0, 0, 1);
    // 做叉乘，得到希望的瞄准方向对应的坐标系的y轴
    tf2::Vector3 y_axis = odom_z_axis.cross(aiming_to);
    // 再做一次叉乘，得到z方向
    tf2::Vector3 z_axis = aiming_to.cross(y_axis);
    // 得到旋转矩阵
    tf2::Matrix3x3 aim_matrix;
    aim_matrix.setValue(aiming_to.x(), y_axis.x(), z_axis.x(),
                        aiming_to.y(), y_axis.y(), z_axis.y(),
                        aiming_to.z(), y_axis.z(), z_axis.z());
    // 计算旋转矩阵的欧拉角
    double roll, pitch, yaw;
    aim_matrix.getRPY(roll, pitch, yaw);
    // it is the same as our robot's zyx euler angle, it is xyz fixed angle
    // get the transform of gimbal_link frame and its r p y
    geometry_msgs::msg::TransformStamped transformStamped;
    
    try{
      transformStamped = tf_buffer_->lookupTransform("odom", "gimbal_link", tf2::TimePointZero/*ros::Time(0)*/);
    }
    catch (tf2::TransformException &ex) {
      RCLCPP_WARN(this->get_logger(),"%s",ex.what());
    }
    // RCLCPP_INFO(this->get_logger(), "transform gotten: q: %f %f %f %f", transformStamped.transform.rotation.x, transformStamped.transform.rotation.y, transformStamped.transform.rotation.z, transformStamped.transform.rotation.w);

    double gim_roll, gim_pitch, gim_yaw;
    tf2::Quaternion q(
      transformStamped.transform.rotation.x,
      transformStamped.transform.rotation.y,
      transformStamped.transform.rotation.z,
      transformStamped.transform.rotation.w);
    tf2::Matrix3x3 m(q);
    m.getRPY(gim_roll, gim_pitch, gim_yaw);
    // log
    // RCLCPP_INFO(this->get_logger(), "gimbal_link rpy: %f %f %f, aim rpy: %f %f %f", gim_roll, gim_pitch, gim_yaw, roll, pitch, yaw);

    // Use the roll, pitch, and yaw angles here
    float space_bullet_delta_pitch = pitch - gim_pitch;
    final_delta_yaw = yaw - gim_yaw;

    // watch that the pitch for algorithm gimbal link is opposite to electric control C-board pitch
    space_bullet_delta_pitch = -space_bullet_delta_pitch;

    /* pitch 抬升为正方向，和电控统一 */
    /* 此时已经让云台link 指向了目标 */
    /* 接下来是补偿枪口下坠 */

    
    // log
    #ifdef CALIBERATING_GUN
    // “太空”子弹的发射角度，一个直线运动、不下坠的子弹
    RCLCPP_INFO(this->get_logger(), "[no_calib][space bullet] delta pitch: %f, delta yaw: %f, dis: %f", space_bullet_delta_pitch, final_delta_yaw, dis);
    #endif

    // 我们认为下坠补偿来源于两个方面，一个是子弹下坠，一个是电机PID控制的静态误差
    // 当补偿了子弹下坠后的delta pitch和delta yaw小于一定数值时，即可发射
    // 在补偿了子弹下坠后，进一步补偿电机静态误差，再将控制量发给电机
    // pitch compensate: bullet dropping

    float pitch_bullet_compensate = earth_comp_k_ * dis + earth_comp_b_;
    // 拟合曲线得到，在步兵上，27.2 m/s的射速

    float earth_bullet_delta_pitch = space_bullet_delta_pitch - pitch_bullet_compensate;

    #ifdef CALIBERATING_GUN
    // “地球”子弹的发射角度。实际上，下坠的补偿，也包含了一些机械结构上的静态误差，例如相机位置有小幅度偏差
    RCLCPP_INFO(this->get_logger(), "[after bullet droppint comp][earth bullet] delta pitch: %f, delta yaw: %f, dis: %f", earth_bullet_delta_pitch, final_delta_yaw, dis);
    #endif

    // 发射判据：反小陀螺模式下的判据和正常模式下的判据，应当是三维空间中的距离信息
    // 计算判据所使用的角度，是在bullet dropping修正后的角度。
    // 唯一的判断标准：“未来装甲板”是否已经在射击范围内
    // 是否已经达到了地球子弹的发射角度
    // 用这两个delta乘以距离（角度比较小，sin theta, tan theta和theta都是同样的了 ）

    // 装甲板的半高，单位m
    if(abs(earth_bullet_delta_pitch * dis) < 0.067 && abs(final_delta_yaw * dis) < 0.067 /*对于大装甲板，应当是0.112，这里暂时统一写成小装甲板的长度，后续有时间再改*/)
      final_shoot = 1;
    else
      final_shoot = 0;

    // 其实还应当考虑太远的目标，直接不打了，可能中不了
    // if(!(dis < 7.0))
    //   final_shoot = 0;
    

    // 静态误差系数，通过观察pitch输出测量
    // float static_error_electric_mechanics = 0.03;

    final_delta_pitch = earth_bullet_delta_pitch + static_error_electric_mechanics_pitch_;
    final_delta_yaw -= static_error_electric_mechanics_yaw_;
    #ifdef CALIBERATING_GUN
    // 为了补偿电控-机械结构静态误差而在发送时提供的增量
    RCLCPP_INFO(this->get_logger(), "[after static error comp][real send] delta pitch: %f, delta yaw: %f, dis: %f", final_delta_pitch, final_delta_yaw, dis);
    #endif

    time_point<high_resolution_clock> nowtime = std::chrono::system_clock::now();
    int64_t micro_seconds_passed = duration_cast<std::chrono::microseconds>(nowtime -lasttime).count();
    int64_t milli_seconds_passed = micro_seconds_passed / 1000;
    if(milli_seconds_passed > shoot_gap_) {
      final_shoot = 1;
      lasttime = nowtime;
    } else {
      final_shoot = 0;
    }

    // 发布控制信息
    auto_aim_interfaces::msg::DangleShoot dangle_shoot_message;
    dangle_shoot_message.delta_pitch = final_delta_pitch;
    dangle_shoot_message.delta_yaw = final_delta_yaw;
    dangle_shoot_message.shoot = final_shoot;
    float_ctr_publisher_->publish(dangle_shoot_message);
  } else {
    // not tracking any target, send 0
    final_shoot = 0;
    final_delta_pitch = static_error_electric_mechanics_pitch_; // 实际上这会导致云台缓慢下降，更好的方式是：让云台瞄准前方某一个可能出现装甲板或者更容易看到装甲板的位置
    final_delta_yaw = 0;
  }

}


void AntiSpinner::decideFollow(const auto_aim_interfaces::msg::Target::SharedPtr msg)
{
  // info that we send

  const static std::map<std::string, uint8_t> id_unit8_map{
    {"", 0},  {"outpost", 0}, {"1", 1}, {"1", 1},     {"2", 2},
    {"3", 3}, {"4", 4},       {"5", 5}, {"guard", 6}, {"base", 7}};
  
  std_msgs::msg::Float64 latency;
  latency.data = (this->now() - msg->header.stamp).seconds() * 1000.0;

  predict_and_pub_ctr(msg, (float)(latency.data));


  RCLCPP_DEBUG_STREAM(get_logger(), "Total latency: " + std::to_string(latency.data) + "ms");
  latency_pub_->publish(latency);

}

void AntiSpinner::getParams()
{
  // using FlowControl = drivers::serial_driver::FlowControl;
  // using Parity = drivers::serial_driver::Parity;
  // using StopBits = drivers::serial_driver::StopBits;

  // uint32_t baud_rate{};
  // auto fc = FlowControl::NONE;
  // auto pt = Parity::NONE;
  // auto sb = StopBits::ONE;

  try {
    earth_comp_k_ = declare_parameter<float>("earth_comp_k", 0.0);
    earth_comp_b_ = declare_parameter<float>("earth_comp_b", 0.0);
    static_error_electric_mechanics_pitch_ = declare_parameter<float>("static_error_electric_mechanics_pitch", 0.0);
    static_error_electric_mechanics_yaw_ = declare_parameter<float>("static_error_electric_mechanics_yaw", 0.0);
    shoot_action_delay_ = declare_parameter<float>("shoot_action_delay", 0.0);
    shoot_speed_ = declare_parameter<float>("shoot_speed", 0.0);
    critical_omega_ = declare_parameter<float>("critical_omega", 0.0);
    shoot_gap_ = declare_parameter<float>("shoot_gap", 0.0);

    RCLCPP_INFO(get_logger(), "gun caliberation coef:");
    RCLCPP_INFO(get_logger(), "earth_comp_k: %f, earth_comp_b:%f, static_error_electric_mechanics: %f shoot_action_delay_: %f, shoot_speed_: %f, critical_omega: %f", 
      earth_comp_k_, earth_comp_b_, static_error_electric_mechanics_pitch_,
      shoot_action_delay_, shoot_speed_, critical_omega_);
    
  } catch (rclcpp::ParameterTypeException & ex) {
    RCLCPP_ERROR(get_logger(), "gun caliberation coef not set properly");
    throw ex;
  }

}

void AntiSpinner::setParam(const rclcpp::Parameter & param)
{
  if (!detector_param_client_->service_is_ready()) {
    RCLCPP_WARN(get_logger(), "Service not ready, skipping parameter set");
    return;
  }

  if (
    !set_param_future_.valid() ||
    set_param_future_.wait_for(std::chrono::seconds(0)) == std::future_status::ready) {
    RCLCPP_INFO(get_logger(), "Setting detect_color to %ld...", param.as_int());
    set_param_future_ = detector_param_client_->set_parameters(
      {param}, [this, param](const ResultFuturePtr & results) {
        for (const auto & result : results.get()) {
          if (!result.successful) {
            RCLCPP_ERROR(get_logger(), "Failed to set parameter: %s", result.reason.c_str());
            return;
          }
        }
        RCLCPP_INFO(get_logger(), "Successfully set detect_color to %ld!", param.as_int());
        initial_set_param_ = true;
      });
  }
}

void AntiSpinner::resetTracker()
{
  if (!reset_tracker_client_->service_is_ready()) {
    RCLCPP_WARN(get_logger(), "Service not ready, skipping tracker reset");
    return;
  }

  auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
  reset_tracker_client_->async_send_request(request);
  RCLCPP_INFO(get_logger(), "Reset tracker!");
}


}  // namespace anti_spinner

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(anti_spinner::AntiSpinner)
